/*
 * CameraStateValidityChecker.h
 *
 *  Created on: Mar 22, 2013
 *      Author: ohnozzy
 */
#ifndef CAMERASTATEVALIDITYCHECKERM_H_
#define CAMERASTATEVALIDITYCHECKERM_H_
#include <vector>
#include <utility>
#include <flyingcamera/base/CameraStateValidityChecker.h>
#include <fcl/collision.h>
#include <fcl/shape/geometric_shapes.h>
#include "flyingcamera/base/CameraSpace.h"
#include "flyingcamera/base/VehiclePath.h"
namespace flyingcamera
{
    namespace base
    {
    	namespace moving
    	{
    		class CameraStateValidityCheckerM : public flyingcamera::base::CameraStateValidityChecker
    		{
    			protected:
    			double time_; //start time
    			double delta_t; //velocity time unit
    			std::vector<fcl::CollisionGeometry*> m_obstacles;
    			std::vector<fcl::Transform3f> m_obstacles_tf;
    			std::vector<std::pair< double, double > > m_obstacles_velocity;

    			public:
    			CameraStateValidityCheckerM(const ompl::base::SpaceInformationPtr &si, VehiclePath* p, double camera_distance, double base_r) :
                    	flyingcamera::base::CameraStateValidityChecker(si, p, camera_distance, base_r), time_(0), delta_t(0.001)
    			{}
    			virtual bool isValid(const ompl::base::State *state) const;
    			void addObstacle(fcl::CollisionGeometry* const o, const fcl::Transform3f&  tf);
    			void addMObstacle(fcl::CollisionGeometry* const obstacle, const fcl::Transform3f&  tf, const std::pair< double, double > velocity);
    			void removeMObstacles();
    			void setStartTime(double time);
    			double getStartTime();
    		};
    	}
    }
}
#endif
